#include "TablePlane.h"

TablePlane::TablePlane(unsigned int minIter, unsigned int maxIter, double sigma, 
		       unsigned int minSamples, bool horizontalAssumption) :
  RANSACproblem< Point3DCol, std::vector<double> >(minIter, maxIter, sigma, minSamples) {
  _doReestimation = true;
  _horizontalAssumption = horizontalAssumption;
}

TablePlane::~TablePlane() {
}

bool TablePlane::fitModelToData(std::vector<Point3DCol*> &data) { 
  unsigned int n = data.size();
  double a,b,c,d;
  
  if(n>3) {
    //std::cout << "N>3" << std::endl;
    double **points = new double* [n];
    for (unsigned int i=0;i<n;i++){
      points[i] = data[i]->pos; 
    }
    
    //Fits a plane Ax+By+Cz+D=0 to the given points
    fitPlane(points, n, a, b, c, d);
    
    estimate->at(0) = a;
    estimate->at(1) = b;
    estimate->at(2) = c;
    estimate->at(3) = d;
    

    delete[] points;
    return true;
    
  }
  else if(n==3){
    double x1,y1,z1,x2,y2,z2,x3,y3,z3;
    
    x1 = data[0]->pos[0];
    y1 = data[0]->pos[1];
    z1 = data[0]->pos[2];
    x2 = data[1]->pos[0];
    y2 = data[1]->pos[1];
    z2 = data[1]->pos[2];
    x3 = data[2]->pos[0];
    y3 = data[2]->pos[1];
    z3 = data[2]->pos[2];
    
    a = y1*(z2 - z3) + y2*(z3 - z1) + y3*(z1 - z2);
    b = z1*(x2 - x3) + z2*(x3 - x1) + z3*(x1 - x2);
    c = x1*(y2 - y3) + x2*(y3 - y1) + x3*(y1 - y2);
    d = -1*(x1*(y2*z3 - y3*z2) + x2*(y3*z1 - y1*z3) + x3*(y1*z2 - y2*z1));
            

    estimate->at(0) = a;
    estimate->at(1) = b;
    estimate->at(2) = c;
    estimate->at(3) = d;
    return true;
  }
  return false;
}
 
void TablePlane::getBestConsensusSet() {
  consensusSet.clear();

  double diff;
  double a = bestEstimate->at(0);
  double b = bestEstimate->at(1);
  double c = bestEstimate->at(2);
  double d = bestEstimate->at(3);
  double n = sqrt(a*a + b*b + c*c);

  //run through all points in set
  for(unsigned int i=0; i<inputSet->size(); i++) {
    // The difference between real value and value based on estimate
    diff = fabs( a*inputSet->at(i)->pos[0] + b*inputSet->at(i)->pos[1] + c*inputSet->at(i)->pos[2] + d ) / n;
                
    // if the distance towards the plane is small enough, we add it to consensusSet
    if (diff <= _sigma )
      consensusSet.push_back(inputSet->at(i));
  }

  //printf("Nr of points in the consensus set: %d of %d\n", (int)consensusSet.size(), (int)inputSet->size());

}

double TablePlane::getFitScore() {
  unsigned int inputSetSize = inputSet->size();
        
  double diff;
        
  int nrInliers = 0;
  int nrOutliers = 0;
        
  double a = estimate->at(0);
  double b = estimate->at(1);
  double c = estimate->at(2);
  double d = estimate->at(3);
  double n = sqrt(a*a + b*b + c*c);

  if(_horizontalAssumption && ( fabs(c) < 2*fabs(a) || fabs(c) < 2*fabs(b) )) {
    return inputSetSize; // zero inliers
  }
  else {
    //run through all points in set
    for(unsigned int i=0; i<inputSetSize; i++) {
      // The difference between real value and value based on estimate
      diff = fabs( a * inputSet->at(i)->pos[0] + b * inputSet->at(i)->pos[1] + c * inputSet->at(i)->pos[2] + d ) / n;
      
      // if the distance towards the plane is small enough, we add it to consensusSet
      if (diff <= _sigma)
	nrInliers++;
      else
	nrOutliers++;
    }
  }

  //printf("Fitscore: %f, %f, %f, %f  ->  %d\n", a, b, c, d, inputSetSize - nrInliers); 
  //std::cout << "Size of consensus set: " << consensusSet->size() << std::endl;
  return inputSetSize - nrInliers;
}

void TablePlane::printBestEstimate() 
{
  std::cout << "Best plane estimation -> " << bestEstimate->at(0) << "," <<
    bestEstimate->at(1) << "," <<
    bestEstimate->at(2) << "," <<
    bestEstimate->at(3) << std::endl;

}

std::vector<double> TablePlane::getTablePlaneVector() {
  std::vector<double> tpv;
  for(unsigned int i=0; i<4; i++)
    tpv.push_back( bestEstimate-> at(i) );

  return tpv;
}

std::vector<double> TablePlane::getTablePlaneNormal() {
  std::vector<double> tablePlaneNormal(3);
  double length = 0.0;
  for(unsigned int i=0; i<3; i++) {
    tablePlaneNormal[i] = bestEstimate->at(i);
    length += tablePlaneNormal[i]*tablePlaneNormal[i];
  }
  if(length>0) {
    for(unsigned int i=0; i<3; i++) 
      tablePlaneNormal[i] /= sqrt(length);
  }
  else {
    for(unsigned int i=0; i<3; i++) 
      tablePlaneNormal[i] = 0.0;
  }
  return(tablePlaneNormal);
}


#ifdef SVD_USING_MAC_ACCELERATE
// Modified from CoViS project
int TablePlane::planeFitHelpMac(int &rowsL,int &colsL,int &rows, double *Mt,double &pA,double &pB,double &pC,double &pD) {
  
  if( colsL != 4 ) {
    printf("Data is not 4D\n");
    return 0;
  }
  if( rowsL < 3 ) {
    printf("Too few points supplied for plane fitting\n");
    return 0;
  }
  
  /*
   *   % Set up constraint equations of the form  AB = 0,
   *   % where B is a column vector of the plane coefficients
   *   % in the form   b(1)*X + b(2)*Y +b(3)*Z + b(4) = 0.
   *
   *   A = [XYZ' ones(npts,1)]; % Build constraint matrix
   */
  
  // Perform Singular Value Decomponsition to fit a plane through the points

  // Define the U, Sigma and V^t
  double *u = new double[1];  // U is not actually calculated to reduce the amount of memory used. Therefore we set it to a size of 1 
  double *sigma = new double[4];
  double *vt = new double[4*4];
  
  char jobU = 'N', jobVT = 'A';
  int rowsR = rowsL;
  int ldu = 4;
  //int ldu = rowsL;
  int ldvt = 4;
  int lwork = 5 * rowsL;
  int info;
  double *w = new double[lwork];
  dgesvd_(&jobU, &jobVT, &rowsL, &colsL, Mt, &rowsR, sigma, u, &ldu, vt, &ldvt, w, &lwork, &info);

  pA = vt[3 + 0];
  pB = vt[3 + 1*4];
  pC = vt[3 + 2*4];
  pD = vt[3 + 3*4];
  
  delete[] u;
  delete[] sigma;
  delete[] vt;

  return 1;
}

// Code copied from CoViS project
// Fits plane to the points supplied in XYZ
// Adapted from matlab file written by Peter Kovesi
// Different from the matlab file, points are supplied as:
//           x   y   z
//           x'  y'  z'
//           x"  y"  z"
//           .........
int TablePlane::fitPlaneMac(double ** XYZ, int rows,double &pA,double &pB,double &pC,double &pD,double divisionFactor) {
  int rowsL, colsL;
  
  rowsL = rows;
  colsL = 4;
  
  if( rowsL == 3)
    rowsL = 4;
    
  // NB. LAPACK uses column-based matrices
  double *Mt = new double[rowsL*4];
  for(int j=0; j < colsL; j++) {	    
    for (int i=0; i < rows; i++) {
      if(j<3)
	Mt[j*rows+i] = XYZ[i][j]*divisionFactor;
      else
	Mt[j*rows+i] = 1.0;
    }
  }

  planeFitHelp(rowsL,colsL,rows,Mt,pA,pB,pC,pD);
  
  return 1;
}
#endif

#ifdef SVD_USING_BOOST_BINDINGS
int TablePlane::planeFitHelp(int &rowsL,int &colsL,int &rows,boost::numeric::ublas::matrix<double> &Mt,double &pA,double &pB,double &pC,double &pD) {
	namespace lapack = boost::numeric::bindings::lapack;
	typedef boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> KColumnMatrix;

	if( colsL != 3 ) {
		printf("Data is not 3D\n");
		return 0;
	}
	if( rowsL < 3 ) {
		printf("Two few points supplied for plane fitting\n");
		return 0;
	}

	/*
	 *   % Set up constraint equations of the form  AB = 0,
	 *   % where B is a column vector of the plane coefficients
	 *   % in the form   b(1)*X + b(2)*Y +b(3)*Z + b(4) = 0.
	 *
	 *   A = [XYZ' ones(npts,1)]; % Build constraint matrix
	 */

	// Define the U, Sigma and V^t
	//KColumnMatrix u(rowsL, rowsL);
	KColumnMatrix u(1,1); // Not calculated, so only contains one element
	boost::numeric::ublas::vector<double> sigma(4);
	KColumnMatrix vt(4, 4);

	if( rows == 3 ) {
		Mt (rows,0) = 0.0;
		Mt (rows,1) = 0.0;
		Mt (rows,2) = 0.0;
		Mt (rows,3) = 0.0;
	}
	KColumnMatrix Mc(Mt);
	size_t lw;
	lw = lapack::gesvd_work ('M', 'A', 'A', Mc);
	#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
	lw = lapack::gesvd_work ('O', 'A', 'A', Mc);
	#endif
	std::vector<double> w (lw);
	//lapack::gesvd ('A', 'A', Mc, sigma, u, vt, w);
	lapack::gesvd ('N', 'A', Mc, sigma, u, vt, w);
	//lapack::dgesvd ('S', 'A', rowsL, colsL, Mc, 4, sigma, u, vt, w);

	//for(int i=0; i<4; i++) {
	//  for(int j=0; j<4; j++) {
	//    std::cout << vt(i,j) << ", ";
	//  }
	//  std::cout << std::endl;
	//}
	//std::cout << std::endl;

	pA = vt(3,0);
	pB = vt(3,1);
	pC = vt(3,2);
	pD = vt(3,3);

	return 1;
}

// Code copied from CoViS project
// Fits plane to the points supplied in XYZ
// Adapted from matlab file written by Peter Kovesi
// Different from the matlab file, points are supplied as:
//           x   y   z
//           x'  y'  z'
//           x"  y"  z"
//           .........
int TablePlane::fitPlane(double ** XYZ, int rows,double &pA,double &pB,double &pC,double &pD,double divisionFactor) {
	namespace lapack = boost::numeric::bindings::lapack;
	//typedef boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> KColumnMatrix;
	int rowsL, colsL;

	rowsL = rows;
	colsL = 3;
	
	if( rowsL == 3)
	  rowsL = 4;

	//printf("rowsL: %d (%d)...", rowsL, rows);
	boost::numeric::ublas::matrix<double> Mt(rowsL,4);

	for ( int i = 0; i < rows; i++)
		for (unsigned int j = 0; j < 4; j++) {
			Mt (i,j) = 1.0;
			if (j<3)
				Mt (i,j) = XYZ[i][j]*divisionFactor;
		}
	//printf(" plane fit...\n");
	planeFitHelp(rowsL,colsL,rows,Mt,pA,pB,pC,pD);
	//printf(" done\n");
	return 1;
}
#endif
